/*
 * manipulator.c
 *
 *  Created on: Dec 4, 2023
 *      Author: boen
 */

#include <manipulator.h>


#define GET_LOW_BYTE(A) ((uint8_t)(A))
//宏函数 获得A的低八位
#define GET_HIGH_BYTE(A) ((uint8_t)((A) >> 8))
//宏函数 获得A的高八位
#define BYTE_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))
//宏函数 将高低八位合成为十六位

static uint8_t tx_buffer[10];
static uint8_t rx_buffer[SERVO_RX_BUFF_MAX];

static uint16_t act_pulse = APP_NULL;
static int step_rx_run = APP_NULL;
static int step_tx_run = APP_NULL;

static unsigned int servo_tick = SERVO_RUNTIME_MAX+1000;


int turntable = 500;

/**
  * @brief
  * @param
  * @retval
  */
void serial_servo_timer(void)
{
	if(servo_tick)
	{
		servo_tick--;
	}
}


/**
* @brief
* @param
* @retval
*/
int serial_servo_get_reply(uint8_t buffer[SERVO_RX_BUFF_MAX])
{
	memcpy(rx_buffer,buffer,SERVO_RX_BUFF_MAX);
	return 0;
}


/**
* @brief
* @param
* @retval
*/
static inline int rad2pulse(float rad)
{
	float tmp = rad*RAD_PROP+500.0f;
	int pulse = (int)tmp;
	if (pulse>1000)
	{
		pulse = 1000;
		//printf("[warn]: The angle is greater than 120(deg) \r\n");
	}

	if (pulse<0)
	{
		pulse = 0;
		//printf("[warn]: The angle is less than 120(deg) \r\n");
	}

	return pulse;
}


/**
* @brief
* @param
* @retval
*/
static inline float pulse2rad(uint16_t pulse)
{
	float rad = pulse;
	rad = ((rad-500.0f)/RAD_PROP);
	return rad;
}


/**
* @brief
* @param
* @retval
*/
static inline int meter2pulse(float meter)
{
	float tmp = 660.0f - meter*MET_PROP;
	int pulse = (int)tmp;
	if (pulse>660)
	{
		pulse = 660;
	}

	if (pulse<0)
	{
		pulse = 0;
	}

	return pulse;
}


/**
* @brief
* @param
* @retval
*/
static inline float pulse2meter(uint16_t pulse)
{
	float meter = pulse;
	meter = (meter-660.0f)/MET_PROP;
	return meter;
}


/**
* @brief
* @param
* @retval
*/
static inline void serial_servo_write(const uint8_t *buffer, uint16_t length, uint8_t it)
{
    HAL_GPIO_WritePin(SERIAL_SERVO_RX_EN_GPIO_Port, SERIAL_SERVO_RX_EN_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(SERIAL_SERVO_TX_EN_GPIO_Port, SERIAL_SERVO_TX_EN_Pin, GPIO_PIN_RESET);
    if(it)
    {
    	HAL_UART_Transmit_IT(&SERVO_PORT, (void *) buffer, length);
    }
    else
    {
    	HAL_UART_Transmit(&SERVO_PORT, (void *) buffer, length, 1000);
    }
    //HAL_Delay(2u);
}


/**
* @brief
* @param
* @retval
*/
static inline uint8_t serial_servo_checksum(const uint8_t buf[])
{
    uint16_t temp = 0;
    for (int i = 2; i < buf[3] + 2; ++i) {
        temp += buf[i];
    }
    return (uint8_t)(~temp);
}


/**
* @brief
* @param
* @retval
*/
static inline void serial_servo_set_position(uint8_t servo_id, uint16_t position, uint16_t duration)
{
    if (servo_id > 31)
    {
        return;
    }

    position = position > 1000 ? 1000 : position;
    tx_buffer[0] = SERVO_FRAME_HEADER;
    tx_buffer[1] = SERVO_FRAME_HEADER;
    tx_buffer[2] = servo_id;
    tx_buffer[3] = 7;
    tx_buffer[4] = SERVO_MOVE_TIME_WRITE;
    tx_buffer[5] = GET_LOW_BYTE(position);
    tx_buffer[6] = GET_HIGH_BYTE(position);
    tx_buffer[7] = GET_LOW_BYTE(duration);
    tx_buffer[8] = GET_HIGH_BYTE(duration);
    tx_buffer[9] = serial_servo_checksum(tx_buffer);
#if 0
    printf("[dbg]: set_position write:[");
    for(int i=0; i<10; i++)
    {
    	printf(" %02x ",tx_buffer[i]);
    }
    printf("]\r\n");
#endif
    serial_servo_write(tx_buffer, 10, 0);
}


/**
* @brief
* @param
* @retval
*/
static inline void serial_servo_get_position(uint8_t servo_id)
{
    if (servo_id > 31)
    {
        return;
    }
    tx_buffer[0] = SERVO_FRAME_HEADER;
    tx_buffer[1] = SERVO_FRAME_HEADER;
    tx_buffer[2] = servo_id;
    tx_buffer[3] = 3;
    tx_buffer[4] = SERVO_POS_READ;
    tx_buffer[5] = serial_servo_checksum(tx_buffer);
#if 0
    printf("[dbg]: get_position write:[");
    for(int i=0; i<6; i++)
    {
    	printf(" %02x ",tx_buffer[i]);
    }
    printf("]\r\n");
#endif
    serial_servo_write(tx_buffer, 6, 1);
}


/**
* @brief
* @param
* @retval
*/
int manipulator_is_ready(void)
{
	if(xbot.manipulator.state == SYS_FLAG_IDLE)
	{
		return 1;
	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int manipulator_set_joint(float j1, float j2, float j3, float j4)
{
	static int step = 0;
	switch(step)
	{
	case 0:
		if(xbot.manipulator.state == SYS_FLAG_IDLE)
		{
			xbot.manipulator.servo[PHY_SERVO_1].fPrePOS = j1;
			xbot.manipulator.servo[PHY_SERVO_2].fPrePOS = j2;
			xbot.manipulator.servo[PHY_SERVO_3].fPrePOS = j3;
			xbot.manipulator.servo[PHY_SERVO_4].fPrePOS = j4;
			xbot.manipulator.state = SYS_FLAG_PLAY;
			step_tx_run = 0;
			step = 1;
		}
		break;
	case 1:
		if(xbot.manipulator.state == SYS_FLAG_IDLE)
		{
			step = 0;
			return 1;
		}
		break;
	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int manipulator_set_pose(float x, float y, float z, float roll, float pitch, float yaw)
{
	static int step = 0;
	switch(step)
	{
		case 0:
			if(xbot.manipulator.state == SYS_FLAG_IDLE)
			{
				if(arm_ik(x, y, z,
					&xbot.manipulator.servo[PHY_SERVO_1].fPrePOS,
					&xbot.manipulator.servo[PHY_SERVO_2].fPrePOS,
					&xbot.manipulator.servo[PHY_SERVO_3].fPrePOS,
					&xbot.manipulator.servo[PHY_SERVO_4].fPrePOS))
				{
					xbot.manipulator.state = SYS_FLAG_PLAY;
					step_tx_run = 0;
					step = 1;
				}
			}
			break;
		case 1:
			if(xbot.manipulator.state == SYS_FLAG_IDLE)
			{
				step = 0;
				return 1;
			}
			break;
	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int manipulator_set_gripper(float state)
{
	static int step = 0;
	switch(step)
	{
		case 0:
			if(xbot.manipulator.state == SYS_FLAG_IDLE)
			{
				xbot.manipulator.gripper = meter2pulse(state);;
				servo_tick = SERVO_RUNTIME_MAX;
				xbot.manipulator.state = SYS_FLAG_PLAY;
				step_tx_run = 5u;
				step = 1;
			}
			break;
		case 1:
			if(xbot.manipulator.state == SYS_FLAG_IDLE)
			{
				step = 0;
				return 1;
			}
			break;
	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int manipulator_set_turntable(uint8_t state)
{
	static int step = 0;
	switch(step)
	{
		case 0:
			if(xbot.manipulator.state == SYS_FLAG_IDLE)
			{
				switch(state)
				{
					case 1:
						turntable = 500u;
						break;
					case 2:
						turntable = 0u;
						break;
					case 3:
						turntable = 1000u;
						break;
					default:
						turntable = 500u;
						break;
				}
				servo_tick = SERVO_RUNTIME_MAX;
				xbot.manipulator.state = SYS_FLAG_PLAY;
				step_tx_run = 6u;
				step = 1;
			}
			break;
		case 1:
			if(xbot.manipulator.state == SYS_FLAG_IDLE)
			{
				step = 0;
				return 1;
			}
			break;
	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int manipulator_init(void)
{
	xbot.manipulator.state = SYS_FLAG_STARTUP;

	xbot.manipulator.pose.position.X = 0.0f;			// X轴偏移量
	xbot.manipulator.pose.position.Y = 0.0f;   			// Y轴偏移量
	xbot.manipulator.pose.position.Z = 0.0f;   			// Z轴偏移量
	xbot.manipulator.pose.angular.X = 0.0f;				// X轴旋转量(roll)
	xbot.manipulator.pose.angular.Y = 0.0f;    			// Y轴旋转量(pitch)
	xbot.manipulator.pose.angular.Z = 0.0f;    			// Z轴旋转量(yaw)
	xbot.manipulator.servo[PHY_SERVO_1].fPrePOS = 0.0f;	// 一关节期盼角度
	xbot.manipulator.servo[PHY_SERVO_2].fPrePOS = 0.0f;	// 二关节期盼角度
	xbot.manipulator.servo[PHY_SERVO_3].fPrePOS = 0.0f;	// 三关节期盼角度
	xbot.manipulator.servo[PHY_SERVO_4].fPrePOS = 0.0f;	// 四关节期盼角度
	xbot.manipulator.servo[PHY_SERVO_1].fActPOS = 0.0f;	// 一关节实际角度
	xbot.manipulator.servo[PHY_SERVO_2].fActPOS = 0.0f;	// 二关节实际角度
	xbot.manipulator.servo[PHY_SERVO_3].fActPOS = 0.0f;	// 三关节实际角度
	xbot.manipulator.servo[PHY_SERVO_4].fActPOS = 0.0f;	// 四关节实际角度

	xbot.manipulator.state = SYS_FLAG_IDLE;

	return 0;
}


/**
* @brief
* @param
* @retval
*/
int manipulator_deinit(void)
{
	xbot.manipulator.pose.position.X = 0.0f;			// X轴偏移量
	xbot.manipulator.pose.position.Y = 0.0f;   			// Y轴偏移量
	xbot.manipulator.pose.position.Z = 0.0f;   			// Z轴偏移量
	xbot.manipulator.pose.angular.X = 0.0f;				// X轴旋转量(roll)
	xbot.manipulator.pose.angular.Y = 0.0f;    			// Y轴旋转量(pitch)
	xbot.manipulator.pose.angular.Z = 0.0f;    			// Z轴旋转量(yaw)
	xbot.manipulator.servo[PHY_SERVO_1].fPrePOS = 0.0f;
	xbot.manipulator.servo[PHY_SERVO_2].fPrePOS = 0.0f;
	xbot.manipulator.servo[PHY_SERVO_3].fPrePOS = 0.0f;
	xbot.manipulator.servo[PHY_SERVO_4].fPrePOS = 0.0f;
	xbot.manipulator.servo[PHY_SERVO_1].fActPOS = 0.0f;
	xbot.manipulator.servo[PHY_SERVO_2].fActPOS = 0.0f;
	xbot.manipulator.servo[PHY_SERVO_3].fActPOS = 0.0f;
	xbot.manipulator.servo[PHY_SERVO_4].fActPOS = 0.0f;

	xbot.manipulator.state = SYS_FLAG_SHUTDOWN;
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int manipulator_polling(void)
{
	if(xbot.manipulator.state == SYS_FLAG_PLAY)
	{
		switch(step_tx_run)
		{
		case 0:
			//printf("xbot.manipulator.state = SYS_FLAG_PLAY \r\n");
			servo_tick = SERVO_RUNTIME_MAX+1000;
			step_tx_run = 1;
			break;
		case 1:
			serial_servo_set_position(1, rad2pulse(xbot.manipulator.servo[PHY_SERVO_1].fPrePOS), SERVO_RUNTIME_MAX);
			step_tx_run = 2;
			break;
		case 2:
			serial_servo_set_position(2, rad2pulse(xbot.manipulator.servo[PHY_SERVO_2].fPrePOS), SERVO_RUNTIME_MAX);
			step_tx_run = 3;
			break;
		case 3:
			serial_servo_set_position(3, rad2pulse(xbot.manipulator.servo[PHY_SERVO_3].fPrePOS), SERVO_RUNTIME_MAX);
			step_tx_run = 4;
			break;
		case 4:
			serial_servo_set_position(4, rad2pulse(xbot.manipulator.servo[PHY_SERVO_4].fPrePOS), SERVO_RUNTIME_MAX);
			step_tx_run = 7;
			break;
		case 5:
			serial_servo_set_position(8, xbot.manipulator.gripper, 0u);
			step_tx_run = 7;
			break;
		case 6:
			serial_servo_set_position(9, turntable, 0u);
			step_tx_run = 7;
		default:
			if (servo_tick==0)
			{
				step_tx_run = 0u;
				xbot.manipulator.state = SYS_FLAG_IDLE;
				//printf("xbot.manipulator.state = SYS_FLAG_IDLE \r\n");
			}
			break;
		}
	}
	else
	{
		if ((rx_buffer[0]==0x55)&&(rx_buffer[1]==0x55))
		{
			act_pulse = rx_buffer[6];
			act_pulse <<= 8;
			act_pulse += rx_buffer[5];
			rx_buffer[0] = 0;
			rx_buffer[1] = 0;
		}
		switch(step_rx_run)
		{
		case 0:
			serial_servo_get_position(1);
			step_rx_run = 1;
			break;
		case 1:
			serial_servo_get_position(2);
			xbot.manipulator.servo[PHY_SERVO_1].fActPOS = pulse2rad(act_pulse);
			step_rx_run = 2;
			break;
		case 2:
			serial_servo_get_position(3);
			xbot.manipulator.servo[PHY_SERVO_2].fActPOS = pulse2rad(act_pulse);
			step_rx_run = 3;
			break;
		case 3:
			serial_servo_get_position(4);
			xbot.manipulator.servo[PHY_SERVO_3].fActPOS = pulse2rad(act_pulse);
			step_rx_run = 4;
			break;
		case 4:
			serial_servo_get_position(1);
			xbot.manipulator.servo[PHY_SERVO_4].fActPOS = pulse2rad(act_pulse);
			step_rx_run = 1;
			break;
		default:
			step_rx_run = 0;
			break;
		}
	}
	arm_fk(xbot.manipulator.servo[PHY_SERVO_1].fActPOS,
			xbot.manipulator.servo[PHY_SERVO_2].fActPOS,
			xbot.manipulator.servo[PHY_SERVO_3].fActPOS,
			xbot.manipulator.servo[PHY_SERVO_4].fActPOS,
			&xbot.manipulator.pose.position.X,
			&xbot.manipulator.pose.position.Y,
			&xbot.manipulator.pose.position.Z,
			&xbot.manipulator.pose.angular.X,
			&xbot.manipulator.pose.angular.Y,
			&xbot.manipulator.pose.angular.Z);

	return 0;
}

